#include <FOC_Motor.h>

FOC_Motor::FOC_Motor(int motor_pin1, int motor_pin2, int motor_pin3, unsigned long pole_pairs, double voltage_supply, double limit_voltage, AS5600 *sensorp)
{
    this->motor_pin_a = motor_pin1;
    this->motor_pin_b = motor_pin2;
    this->motor_pin_c = motor_pin3;
    this->motor_pwm_channel_a = 0;
    this->motor_pwm_channel_b = 1;
    this->motor_pwm_channel_c = 2;
    this->pole_pairs = pole_pairs;
    this->voltage_supply = voltage_supply;
    this->limit_voltage = limit_voltage;
    this->shaft_angle = 0;
    this->last_time = 0;
    this->target_velocity = 0;
    this->target_angle = 0;
    this->openloop_flag = false;
    this->openloop_handlep = NULL;
    this->closeloop_pos_flag = false;
    this->closeloop_pos_handlep = NULL;
    this->sensorp = sensorp;

    pinMode(motor_pin_a, OUTPUT);
    pinMode(motor_pin_b, OUTPUT);
    pinMode(motor_pin_c, OUTPUT);

    ledcSetup(motor_pwm_channel_a, 30000, 8);
    ledcAttachPin(motor_pin_a, motor_pwm_channel_a);
    ledcWrite(motor_pwm_channel_a, 0);

    ledcSetup(motor_pwm_channel_b, 30000, 8);
    ledcAttachPin(motor_pin_b, motor_pwm_channel_b);
    ledcWrite(motor_pwm_channel_b, 0);

    ledcSetup(motor_pwm_channel_c, 30000, 8);
    ledcAttachPin(motor_pin_c, motor_pwm_channel_c);
    ledcWrite(motor_pwm_channel_c, 0);

    Serial.println("motor init!");
}

double FOC_Motor::_constrain(double data, double min, double max)
{
    return (data > max) ? max : ((data < min) ? min : data);
}

double FOC_Motor::_normalize_angle(double angle)
{
    angle = fmod(angle, 2 * PI);
    return (angle < 0) ? angle + 2 * PI : angle;
}

double FOC_Motor::_shaft2electrical(double angle)
{
    return angle * pole_pairs;
}

PhaseVoltage FOC_Motor::_foc(double uq, double ud, double electrical_angle)
{
    double u_alpha = -uq * sin(electrical_angle) + ud * cos(electrical_angle);
    double u_beta = uq * cos(electrical_angle) + ud * sin(electrical_angle);

    double ua = u_alpha / 2 + this->limit_voltage / 2;
    double ub = (sqrt(3) * u_beta - u_alpha) / 4 + this->limit_voltage / 2;
    double uc = (-u_alpha - sqrt(3) * u_beta) / 4 + this->limit_voltage / 2;

    PhaseVoltage pv(ua, ub, uc);
    return pv;
}

void FOC_Motor::_pwm(PhaseVoltage pv)
{
    double ua_dc = _constrain(pv.ua / this->voltage_supply, 0.0f, 1.0f);
    double ub_dc = _constrain(pv.ub / this->voltage_supply, 0.0f, 1.0f);
    double uc_dc = _constrain(pv.uc / this->voltage_supply, 0.0f, 1.0f);

    ledcWrite(motor_pwm_channel_a, ua_dc * 255);
    ledcWrite(motor_pwm_channel_b, ub_dc * 255);
    ledcWrite(motor_pwm_channel_c, uc_dc * 255);
}

void FOC_Motor::_openloop_rotate()
{
    this->shaft_angle = 0;
    this->last_time = micros();
    double time_rate = 1e-6f;
    while (this->openloop_flag)
    {
        unsigned long current_time = micros();
        shaft_angle = _normalize_angle(shaft_angle + this->target_velocity * (current_time - last_time) * time_rate);
        double current_electrical_angle = _normalize_angle(_shaft2electrical(shaft_angle));
        PhaseVoltage pv = _foc(0.8 * limit_voltage, 0, current_electrical_angle);
        _pwm(pv);
        last_time = current_time;
        vTaskDelay(1);
    }
    vTaskDelete(this->openloop_handlep);
    delete (this->openloop_handlep);
    this->openloop_handlep = NULL;
}

void FOC_Motor::openloop_rotate_start(double velocity)
{
    this->openloop_flag = true;
    this->openloop_handlep = new TaskHandle_t;
    this->target_velocity = velocity;

    xTaskCreate(
        FOC_Motor::_openloop_rotate_wrapper,
        "openloop_rotate",
        10000,
        this,
        3,
        this->openloop_handlep);
    Serial.println("task start!");
}

void FOC_Motor::openloop_rotate_end()
{
    this->openloop_flag = false;
    Serial.println("task end!");
}

void FOC_Motor::_closeloop_pos()
{
    if (this->sensorp == NULL)
    {
        Serial.println("error: no sensor available");
        return;
    }
    uint8_t direction = 1;
    double current_shaft_angle;
    double current_electrical_angle;
    double kp = this->limit_voltage / (PI / 4);
    double uq;
    this->sensorp->read_angle_task_start();
    while (this->closeloop_pos_flag)
    {
        current_shaft_angle = this->sensorp->get_angle() * direction;
        current_electrical_angle = _shaft2electrical(current_shaft_angle);
        double angle_bias = this->target_angle - current_shaft_angle;
        angle_bias = fmod(angle_bias, 2 * PI);
        uq = _constrain(angle_bias * kp, -limit_voltage, limit_voltage);
        PhaseVoltage pv = _foc(uq, 0, current_electrical_angle);
        _pwm(pv);
        vTaskDelay(1);
    }
    this->sensorp->read_angle_task_end();
    vTaskDelete(this->closeloop_pos_handlep);
    delete (this->closeloop_pos_handlep);
    this->closeloop_pos_handlep = NULL;
}

void FOC_Motor::closeloop_pos_start(double target_angle)
{
    this->closeloop_pos_flag = true;
    this->closeloop_pos_handlep = new TaskHandle_t;
    this->target_angle = target_angle;

    xTaskCreate(
        FOC_Motor::_closeloop_pos_wrapper,
        "closeloop_pos",
        10000,
        this,
        2,
        this->closeloop_pos_handlep);
    Serial.println("task start!");
}

void FOC_Motor::closeloop_pos_end()
{
    this->closeloop_pos_flag = false; 
    Serial.println("task end!");
}

void FOC_Motor::calibrate_sensor()
{
    PhaseVoltage pv = _foc(0.5 * limit_voltage, 0, 3 / 2 * PI);
    _pwm(pv);
    delay(1000);
    this->sensorp->set_init_angle();
    pv = _foc(0, 0, 3 / 2 * PI);
    _pwm(pv);
    Serial.println("sensor calibrated!");
}